#include "StdAfx.h"
#include "CollisionController.h"
#include "IScenery.h"
#include "IMovable.h"
#include "PhysicalObject.h"
#include "BoundingVolume.h"


///////////////////////////////////////////////////////////////////////////////

CCollisionController::CCollisionController()
      : m_scenery(NULL)
{
   m_worldBounds.min = D3DXVECTOR3(-100000, -100000, -100000);
   m_worldBounds.max = D3DXVECTOR3(100000, 100000, 100000);
}

///////////////////////////////////////////////////////////////////////////////

CCollisionController::~CCollisionController()
{
   clearScene();
}

///////////////////////////////////////////////////////////////////////////////

void CCollisionController::clearScene()
{
   for (std::list<PhysicalObject*>::iterator it = m_physicalObjects.begin();
      it != m_physicalObjects.end(); it++)
   {
      delete *it;
   }
   m_physicalObjects.clear();

   m_movables.clear();

   m_worldBounds.min = D3DXVECTOR3(-100000, -100000, -100000);
   m_worldBounds.max = D3DXVECTOR3(100000, 100000, 100000);

   m_scenery = NULL;
}

///////////////////////////////////////////////////////////////////////////////

void CCollisionController::setScenery(IScenery& scenery)
{
   m_scenery = &scenery;
}

///////////////////////////////////////////////////////////////////////////////

PhysicalObject& CCollisionController::addMovable(IMovable& movable)
{
   addGhost(movable);

   PhysicalObject* physicalObject = new PhysicalObject(movable);
   m_physicalObjects.push_back(physicalObject);

   return *physicalObject;
}

///////////////////////////////////////////////////////////////////////////////

void CCollisionController::addGhost(IMovable& movable)
{
   m_movables.push_back(&movable);
}

///////////////////////////////////////////////////////////////////////////////

void CCollisionController::removeMovable(IMovable& movable)
{

   for (std::list<PhysicalObject*>::iterator it = m_physicalObjects.begin();
      it != m_physicalObjects.end(); ++it)
   {
      IMovable& m = (*it)->getMovable();
      if ((void*)(&m) == (void*)(&movable)) 
      {
         m_physicalObjects.erase(it);
         break;
      }
   }

   for (std::list<IMovable*>::iterator it = m_movables.begin();
      it != m_movables.end(); ++it)
   {
      if ((void*)(*it) == (void*)(&movable)) 
      {
         m_movables.erase(it);
         break;
      }
   }
}

///////////////////////////////////////////////////////////////////////////////

void CCollisionController::update(float timeElapsed)
{
   for (std::list<PhysicalObject*>::iterator it = m_physicalObjects.begin();
      it != m_physicalObjects.end(); ++it)
   {
      PhysicalObject* object = *it;
      object->update(timeElapsed);
   }

   for (std::list<IMovable*>::iterator it = m_movables.begin();
      it != m_movables.end(); ++it)
   {
      (*it)->update(timeElapsed);

      keepInWorldBound(**it);
      checkCollisionWithScenery(**it);
   }

   for (std::list<PhysicalObject*>::iterator colidorIt = m_physicalObjects.begin();
      colidorIt != m_physicalObjects.end(); ++colidorIt)
   {
      std::list<PhysicalObject*>::iterator obstacleIt = colidorIt;
      obstacleIt++;
      for (; obstacleIt != m_physicalObjects.end(); ++obstacleIt)
      {
         checkInterMovableCollisions(**colidorIt, **obstacleIt);
      }
   }
}

///////////////////////////////////////////////////////////////////////////////

void CCollisionController::keepInWorldBound(IMovable& movable)
{
   AABoundingBox boundingBox = movable.getWorldSpaceBoundingBox();
   
   D3DXVECTOR3 movementVec(0, 0, 0);

   if (boundingBox.min.x < m_worldBounds.min.x) 
   {
      movementVec.x = m_worldBounds.min.x - boundingBox.min.x;
   }
   else if (boundingBox.max.x > m_worldBounds.max.x) 
   {
      movementVec.x = m_worldBounds.max.x - boundingBox.max.x;
   }

   if (boundingBox.min.z < m_worldBounds.min.z) 
   {
      movementVec.z = m_worldBounds.min.z - boundingBox.min.z;
   }
   else if (boundingBox.max.z > m_worldBounds.max.z) 
   {
      movementVec.z = m_worldBounds.max.z - boundingBox.max.z;
   }

   movable.move(movementVec);
}

///////////////////////////////////////////////////////////////////////////////

void CCollisionController::checkCollisionWithScenery(IMovable& movable)
{
   if (m_scenery == NULL) { return; }

   AABoundingBox boundingBox = movable.getWorldSpaceBoundingBox();
   D3DXVECTOR3 shiftVec = m_scenery->getIntersectionRemovalVector(boundingBox);

   movable.move(shiftVec);
}

///////////////////////////////////////////////////////////////////////////////

void CCollisionController::checkInterMovableCollisions(PhysicalObject& colidor, 
                                                       PhysicalObject& obstacle)
{
   AABoundingBox colidorBB = colidor.getMovable().getWorldSpaceBoundingBox();
   AABoundingBox obstacleBB = obstacle.getMovable().getWorldSpaceBoundingBox();

   if (colidorBB.isCollidingWith(obstacleBB))
   {
      // apply forces
      D3DXVECTOR3 colidorForce = colidor.getMovementForce();
      D3DXVECTOR3 obstacleForce = obstacle.getMovementForce();

      float forceVal = D3DXVec3Length(&colidorForce) + D3DXVec3Length(&obstacleForce);
      D3DXVec3Normalize(&colidorForce, &colidorForce);
      D3DXVec3Normalize(&obstacleForce, &obstacleForce);

      obstacle.applyForce(colidorForce * forceVal);
      colidor.applyForce(obstacleForce * forceVal);

      // separate objects so that they don't intersect
      D3DXVECTOR3 vec = obstacleBB.findIntersectionRemovalVector(colidorBB);
      colidor.getMovable().move(vec);
   }
}

///////////////////////////////////////////////////////////////////////////////

void CCollisionController::setWorldBounds(const AABoundingBox& bounds)
{
   m_worldBounds = bounds;
}

///////////////////////////////////////////////////////////////////////////////
